
[t,x]=ode45(@rhs,linspace(0,10,10000),[0,0,0,1,1,0]);
figure;hold on;
plot(x(:,1),x(:,2))
plot(x(:,4),x(:,5))
axis equal

function dxdt=rhs(t,state)
    x=state(1);
    y=state(2);
    theta=state(3);
    xr=state(4);
    yr=state(5);
    thetar=state(6);
    
    vr=1;
    omegar=1;
    k1=1;
    k2=1;
    k3=1;

    qe=[
        cos(theta) sin(theta) 0
        -sin(theta) cos(theta) 0
        0 0 1]*[xr-x;yr-y;thetar-theta];
    xe=qe(1);
    ye=qe(2);
    thetae=qe(3);
    v=vr*cos(thetae)+k1*(xe);
    omega=omegar+k2*vr*ye+k3*vr*sin(thetae);

    dxdt=[
        v*cos(theta)
        v*sin(theta)
        omega
        vr*cos(thetar)
        vr*sin(thetar)
        omegar
    ];
end